/**
  ******************************************************************************
  * @file    lite_dog.c
  * @author  GEEKROS,  site:www.geekros.com
  ******************************************************************************
*/

#include "lite_dog.h"

Lite_Dog_Struct Lite_Dog;

/******************************************************************
  * @ 函数名  ： Lite_Dog_Init
  * @ 功能说明： 机器狗初始化
  * @ 参数    ： NULL 
  * @ 返回值  ： NULL
  *****************************************************************/
void Lite_Dog_Init(void)
{
    // 打开蓝色LED灯
    Led_State("blue", "on");

	//打开扩展板供电电源接口
	Power_24V_State(0, "on");
	
	//设置舵机串口波特率
	Servo_Init(1000000);
	
	//设置舵机类型
	Servo_Type(0, SERVO_SCS);
	
	//设置电池低电压报警阀值，低电量时开发板红色LED灯亮起
	Adc_Low_Voltage(11.4);
	
	//自稳PID初始化
	
	//设置机器狗基本配置
	Lite_Dog.config.servo_len = Lite_Dog_Len;
	Lite_Dog.config.servo_angle_range = 360.f;
	Lite_Dog.config.body_length = 157.f;
	Lite_Dog.config.body_width = 108.f;
	Lite_Dog.config.foot_width = 116.f;
	Lite_Dog.config.thigh_length = 82.f;
	Lite_Dog.config.calf_length = 63.f;
	Lite_Dog.config.joint_offset = 310;
	
	//从Flash中读取上次校准的零位数据
	Lite_Dog.config.zero_position[0] = 511 - Lite_Dog.config.joint_offset;
	Lite_Dog.config.zero_position[1] = 511 - Lite_Dog.config.joint_offset;
	Lite_Dog.config.zero_position[2] = 511 + Lite_Dog.config.joint_offset;
	Lite_Dog.config.zero_position[3] = 511 + Lite_Dog.config.joint_offset;
	Lite_Dog.config.zero_position[4] = 511 + Lite_Dog.config.joint_offset;
	Lite_Dog.config.zero_position[5] = 511 + Lite_Dog.config.joint_offset;
	Lite_Dog.config.zero_position[6] = 511 - Lite_Dog.config.joint_offset;
	Lite_Dog.config.zero_position[7] = 511 - Lite_Dog.config.joint_offset;
	
	//初始化目标位置、姿态角
	Lite_Dog.target.x = 0;
	Lite_Dog.target.z = 100;
	Lite_Dog.target.pitch = 0;
	Lite_Dog.target.roll = 0;
	Lite_Dog.target.yaw = 0;
	
	//初始化当前位置、姿态角
	Lite_Dog.current.x = 0;
	Lite_Dog.current.z = 100;
	Lite_Dog.current.pitch = 0;
	Lite_Dog.current.roll = 0;
	Lite_Dog.current.yaw = 0;
	
	Lite_Dog.gait_time = 0;
	Lite_Dog.gait_cycle = 15;
	Lite_Dog.motion_mode = Lite_Dog_Stop;
	Lite_Dog.gait_mode = Lite_Dog_Pronk;
	Lite_Dog.control_mode = Lite_Dog_Remote_Control;
	Lite_Dog.auto_stable = 0;
	Lite_Dog.auto_recovery = 0;
	
	Lite_Dog.state = false;
	
	Lite_Dog_Stand_Down("stand");
}

/******************************************************************
  * @ 函数名  ： Lite_Dog_Attitude_To_Coordinate
  * @ 功能说明： 机器狗姿态解算为实际坐标系
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Lite_Dog_Attitude_To_Coordinate(const Lite_Dog_Position_Attitude_Struct *pose, float x[], float z[])
{
	float length = Lite_Dog.config.body_length;
	float width = Lite_Dog.config.body_width;
	
	float pitch = pose->pitch * Lite_Dog_Pi / (Lite_Dog.config.servo_angle_range/2); // 俯仰角
	float roll = pose->roll * Lite_Dog_Pi / (Lite_Dog.config.servo_angle_range/2); // 滚转角
	float yaw = pose->yaw * Lite_Dog_Pi / (Lite_Dog.config.servo_angle_range/2); // 航向角
	
	//计算1、2、3、4腿坐标
	x[0] = length/2 - pose->x - (length*cos(pitch)*cos(yaw))/2 - (width*cos(pitch)*sin(yaw))/2;
	z[0] = (width*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)))/2 - pose->z - (length*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)))/2;
	
	x[1] = (length*cos(pitch)*cos(yaw))/2 - pose->x - length/2 - (width*cos(pitch)*sin(yaw))/2;
	z[1] = (width*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)))/2 - pose->z + (length*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)))/2;
	
	x[2] = length/2 - pose->x - (length*cos(pitch)*cos(yaw))/2 + (width*cos(pitch)*sin(yaw))/2;
	z[2] = -pose->z - (width*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)))/2 - (length*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)))/2;
	
	x[3] = (length*cos(pitch)*cos(yaw))/2 - pose->x - length/2 + (width*cos(pitch)*sin(yaw))/2;
	z[3] = -pose->z - (width*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)))/2 + (length*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)))/2;
}

/******************************************************************
  * @ 函数名  ： Lite_Dog_Coordinate_To_Angle
  * @ 功能说明： 机器狗坐标解算为实际舵机角度
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Lite_Dog_Coordinate_To_Angle(const float x[], const float z[], float thigh[], float calf[])
{
	float thigh_length = Lite_Dog.config.thigh_length;
	float calf_length = Lite_Dog.config.calf_length;
	float angle;
	
	//计算1、2、3、4大小腿角度
	angle = sqrt(x[0]*x[0] + z[0]*z[0]);
	calf[0] = acos((angle*angle - thigh_length*thigh_length - calf_length*calf_length)/(2*thigh_length*calf_length));
	thigh[0] = Lite_Dog_Pi - acos(x[0]/angle) - acos((thigh_length*thigh_length + angle*angle - calf_length*calf_length)/(2*thigh_length*angle));
	
	angle = sqrt(x[1]*x[1] + z[1]*z[1]);
	calf[1] = acos((angle*angle - thigh_length*thigh_length - calf_length*calf_length)/(2*thigh_length*calf_length));
	thigh[1] = Lite_Dog_Pi - acos(x[1]/angle) - acos((thigh_length*thigh_length + angle*angle - calf_length*calf_length)/(2*thigh_length*angle));
	
	angle = sqrt(x[2]*x[2] + z[2]*z[2]);
	calf[2] = acos((angle*angle - thigh_length*thigh_length - calf_length*calf_length)/(2*thigh_length*calf_length));
	thigh[2] = Lite_Dog_Pi - acos(x[2]/angle) - acos((thigh_length*thigh_length + angle*angle - calf_length*calf_length)/(2*thigh_length*angle));
	
	angle = sqrt(x[3]*x[3] + z[3]*z[3]);
	calf[3] = acos((angle*angle - thigh_length*thigh_length - thigh_length*thigh_length)/(2*thigh_length*calf_length));
	thigh[3] = Lite_Dog_Pi - acos(x[3]/angle) - acos((thigh_length*thigh_length + angle*angle - calf_length*calf_length)/(2*thigh_length*angle));
}

/******************************************************************
  * @ 函数名  ： Lite_Dog_Data_To_Servo
	* @ 功能说明： 机器狗角度数据发送至舵机
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Lite_Dog_Data_To_Servo(float thigh[], float calf[], uint16_t zero_position[], uint16_t speed)
{
	uint8_t  servo_id[Lite_Dog.config.servo_len];
	uint16_t servo_position[Lite_Dog.config.servo_len];
	uint16_t servo_speed[Lite_Dog.config.servo_len];
	
	int servo_num = sizeof(servo_id)/sizeof(servo_id[0]);
	
	// 角度值转换->弧度->舵机数字信号
	for(int i=0;i<servo_num/2;i++)
	{
		thigh[i] = thigh[i] * (Lite_Dog.config.servo_angle_range/2) / Lite_Dog_Pi * Lite_Dog_Angle_To_Digital;
		calf[i] = calf[i] * (Lite_Dog.config.servo_angle_range/2) / Lite_Dog_Pi * Lite_Dog_Angle_To_Digital;
	}
	
	//设置舵机ID和速度值
	for(int i=0;i<servo_num;i++)
	{
		servo_id[i] = i + 1;
		servo_speed[i] = speed;
	}
	
	// 通过校准后的零位值换算小腿的位置数据
	servo_position[0] = zero_position[0] + (int32_t)calf[0];
	servo_position[1] = zero_position[1] + (int32_t)calf[1];
	servo_position[2] = zero_position[2] - (int32_t)calf[2];
	servo_position[3] = zero_position[3] - (int32_t)calf[3];
	
	// 通过校准后的零位值换算大腿的位置数据
	servo_position[4] = zero_position[4] - (int32_t)thigh[0];
	servo_position[5] = zero_position[5] - (int32_t)thigh[1];
	servo_position[6] = zero_position[6] + (int32_t)thigh[2];
	servo_position[7] = zero_position[7] + (int32_t)thigh[3];
	
	// 发送舵机位置控制数据
	Servo_Write_Position(servo_id, servo_num, servo_position, 0, servo_speed);
}

/******************************************************************
  * @ 函数名  ： Lite_Dog_Motion
  * @ 功能说明： 机器狗运动控制
  * @ 参数    ： *pose 目标位置、姿态
  * @ 参数    ： speed 速度
  * @ 返回值  ： NULL
  *****************************************************************/
void Lite_Dog_Motion(Lite_Dog_Position_Attitude_Struct *pose, uint16_t speed)
{
	uint16_t step_time = speed/10;
	float step[4] = {0};
	if(pose->x != Lite_Dog.current.x)
	{
		step[0] = (pose->x-Lite_Dog.current.x) / step_time;
	}
	if(pose->z != Lite_Dog.current.z)
	{
		step[1] = (pose->z-Lite_Dog.current.z) / step_time;
	}
	if(pose->pitch != Lite_Dog.current.pitch)
	{
		step[2] = (pose->pitch-Lite_Dog.current.pitch) / step_time;
	}
	if(pose->roll != Lite_Dog.current.roll)
	{
		step[3] = (pose->roll-Lite_Dog.current.roll) / step_time;
	}
	
	for(int i=0;i<step_time;i++)
	{
		Lite_Dog.current.x += step[0];
		Lite_Dog.current.z += step[1];
		Lite_Dog.current.pitch += step[2];
		Lite_Dog.current.roll += step[3];
		Lite_Dog_Attitude_To_Coordinate(&Lite_Dog.current, Lite_Dog.foot_coordinate.x, Lite_Dog.foot_coordinate.z);
		Lite_Dog_Coordinate_To_Angle(Lite_Dog.foot_coordinate.x, Lite_Dog.foot_coordinate.z, Lite_Dog.leg_angle.thigh, Lite_Dog.leg_angle.calf);
		Lite_Dog_Data_To_Servo(Lite_Dog.leg_angle.thigh, Lite_Dog.leg_angle.calf, Lite_Dog.config.zero_position, speed);
		LOS_TaskDelay(10);
	}
}

/******************************************************************
  * @ 函数名  ： Lite_Dog_Stand_Down
  * @ 功能说明： 机器狗站立、趴下控制
  * @ 参数    ： char *state 状态类型 可选值stand、down
  * @ 返回值  ： NULL
  *****************************************************************/
void Lite_Dog_Stand_Down(char *state)
{
	if(strcmp(state, "stand")==0)
	{
		Lite_Dog.target.z = 100;
	}
	if(strcmp(state, "down")==0)
	{
		Lite_Dog.target.z = 35;
	}
	Lite_Dog_Motion(&Lite_Dog.target, 800);
	Lite_Dog.state = true;
}

/******************************************************************
  * @ 函数名  ： Lite_Dog_Recovery_Event
  * @ 功能说明： 机器狗翻倒事件
  * @ 参数    ： NULL 
  * @ 返回值  ： NULL
  *****************************************************************/
void Lite_Dog_Recovery_Event(uint16_t zero_position[], uint8_t *auto_recovery)
{
	
}

/******************************************************************
  * @ 函数名  ： Lite_Dog_Stable_Pid
  * @ 功能说明： 机器狗自稳PID计算
  * @ 参数    ： NULL 
  * @ 返回值  ： NULL
  *****************************************************************/
float Lite_Dog_Stable_Pid(uint8_t auto_stable, float pitch_roll, float mpu, Pid_Struct *pid)
{
	pid->kp = 1.5;
	pid->ki = 0.1;
	pid->kd = 0.5;
	if(auto_stable == 1)
	{
		float set_val = 0;
		pid->error = set_val - mpu;
		pid->diff = pid->error - pid->last_error;
		pid->integral += pid->error;
		pid->output = (pid->kp*pid->error) + (pid->kd*pid->diff) + (pid->ki*pid->integral);
		if(pid->integral>100)
		{
			pid->integral = 100;
		}
		if(pid->integral<-100)
		{
			pid->integral = -100;
		}
		pitch_roll = pid->output;
		pid->last_error = pid->error;
		
		return pitch_roll;
	}else{
		return pitch_roll;
	}
}

/******************************************************************
  * @ 函数名  ： Lite_Dog_Trot_Gait
  * @ 功能说明： 机器狗Trot步态时序计算
  * @ 参数    ： NULL 
  * @ 返回值  ： NULL
  *****************************************************************/
void Lite_Dog_Gait_Time(float *gait_time, float t, float n)
{
	if(*gait_time > n * Lite_Dog_Gait_Cycle) {
		*gait_time = 0;
	}else {
		*gait_time = *gait_time + t;
	}	
}

/******************************************************************
  * @ 函数名  ： Lite_Dog_Trot_Gait
  * @ 功能说明： 机器狗Trot步态控制
  * @ 参数    ： NULL 
  * @ 返回值  ： NULL
  *****************************************************************/
void Lite_Dog_Trot_Gait(float gait_time, float step_width, float lift_height, float x[], float z[])
{
	float sigma;
	float zep_b, xep_b; 
	float xep_z; 		
	float xs = -step_width * 0.8f;
	float xf = step_width * 0.2f;
	float r,l = 0;
	
	r = 0; l = 0;
	
	if(gait_time <= Lite_Dog_Gait_Cycle * Lite_Dog_Gait_Duty_Cycle)
	{
		sigma = 2 * Lite_Dog_Pi * gait_time / (Lite_Dog_Gait_Duty_Cycle*Lite_Dog_Gait_Cycle);
		
		zep_b = lift_height * (1-cos(sigma)) / 2;
		xep_b = (xf-xs) * ((sigma-sin(sigma))/(2*Lite_Dog_Pi)) + xs; 
		xep_z = (xs-xf) * ((sigma-sin(sigma))/(2*Lite_Dog_Pi)) + xf;
		
		z[0] = z[0] + zep_b;
		z[1] = z[1];
		z[2] = z[2];
        z[3] = z[3] + zep_b;
		
		x[0] = x[0] + xep_b * r;
		x[1] = x[1] + xep_z * r;
		x[2] = x[2] + xep_z * l;
		x[3] = x[3] + xep_b * l;
	}
	if (gait_time > Lite_Dog_Gait_Duty_Cycle*Lite_Dog_Gait_Cycle && gait_time <= Lite_Dog_Gait_Cycle)
	{
		sigma = 2 * Lite_Dog_Pi * (gait_time-Lite_Dog_Gait_Cycle*Lite_Dog_Gait_Duty_Cycle) / (Lite_Dog_Gait_Duty_Cycle*Lite_Dog_Gait_Cycle);
		
		zep_b = lift_height * (1-cos(sigma)) / 2;
		xep_b = (xf-xs) * ((sigma-sin(sigma))/(2*Lite_Dog_Pi)) + xs;
		xep_z = (xs-xf) * ((sigma-sin(sigma))/(2*Lite_Dog_Pi)) + xf;
		
		z[0] = z[0];
		z[1] = z[1] + zep_b;
		z[2] = z[2] + zep_b;
        z[3] = z[3];
		
		x[0] = x[0] + xep_z * r;
		x[1] = x[1] + xep_b * r;
		x[2] = x[2] + xep_b * l;
		x[3] = x[3] + xep_z * l;
	}
}

/******************************************************************
  * @ 函数名  ： Lite_Dog_Gait
  * @ 功能说明： 机器狗步态控制
  * @ 参数    ： NULL 
  * @ 返回值  ： NULL
  *****************************************************************/
void Lite_Dog_Gait(Lite_Dog_Gait_Mode gait)
{
	switch(gait)
	{
		case Lite_Dog_Trot:
			Lite_Dog_Gait_Time(&Lite_Dog.gait_time, 0.05, 1);
			Lite_Dog_Trot_Gait(Lite_Dog.gait_time, 40, 40, Lite_Dog.foot_coordinate.x, Lite_Dog.foot_coordinate.z);
			break;
		case Lite_Dog_Walk:
			break;
		case Lite_Dog_Pronk:
			break;
		default:
			break;
	}
}

/******************************************************************
  * @ 函数名  ： Lite_Dog_Task
  * @ 功能说明： 机器狗任务
  * @ 参数    ： NULL 
  * @ 返回值  ： NULL
  *****************************************************************/
void Lite_Dog_Task(void)
{
	if(Lite_Dog.state){
		if(Lite_Dog.control_mode == Lite_Dog_Remote_Control) // 遥控模式
		{
			// 翻倒自动恢复站立
			Lite_Dog_Recovery_Event(Lite_Dog.config.zero_position, &Lite_Dog.auto_recovery);
			// 自稳模式
			Lite_Dog.target.pitch = Lite_Dog_Stable_Pid(Lite_Dog.auto_stable, Lite_Dog.target.pitch, Mpu_Read_Data.rol, &Lite_Dog.stable_pitch_pid);
			Lite_Dog.target.roll = Lite_Dog_Stable_Pid(Lite_Dog.auto_stable, Lite_Dog.target.roll, Mpu_Read_Data.pit, &Lite_Dog.stable_roll_pid);
			// 高度控制
			Lite_Dog.current.z = Pid_Dog_Calculation(Lite_Dog.target.z, Lite_Dog.current.z);
			// 重心控制
			Lite_Dog.current.x = Pid_Dog_Calculation(Lite_Dog.target.x, Lite_Dog.current.x);
			// 俯仰控制
			Lite_Dog.current.pitch = Pid_Dog_Calculation(Lite_Dog.target.pitch, Lite_Dog.current.pitch);
			// 滚转控制
			Lite_Dog.current.roll = Pid_Dog_Calculation(Lite_Dog.target.roll, Lite_Dog.current.roll);
			
			// 姿态解算为实际坐标
			Lite_Dog_Attitude_To_Coordinate(&Lite_Dog.current, Lite_Dog.foot_coordinate.x, Lite_Dog.foot_coordinate.z);
			
			// 步态控制
			Lite_Dog_Gait(Lite_Dog.gait_mode);
			
			// 坐标解算为实际舵机角度
			Lite_Dog_Coordinate_To_Angle(Lite_Dog.foot_coordinate.x, Lite_Dog.foot_coordinate.z, Lite_Dog.leg_angle.thigh, Lite_Dog.leg_angle.calf);
			// 角度数据发送至舵机
			Lite_Dog_Data_To_Servo(Lite_Dog.leg_angle.thigh, Lite_Dog.leg_angle.calf, Lite_Dog.config.zero_position, 1500);
		}
		if(Lite_Dog.control_mode == Lite_Dog_Calibration) // 标定模式
		{
			
		}
		if(Lite_Dog.control_mode == Lite_Dog_Action) // 动作模式
		{
			
		}
	}
	
	LOS_TaskDelay(Lite_Dog.gait_cycle);
}
